/*！
 * @comment: 基于RTKLIB的原始观测处理程序，读取原始观测文件，整理成文本矩阵
 * @author: rebeater
 * */

#include <iostream>
#include <fstream>
#include <iomanip>
#include <glog/logging.h>
#include "rtklib.h"
#include "rtklib_custom.h"
#include "gflags/gflags.h"

DEFINE_string(gps,"","gps navigation file");
DEFINE_string(bds,"","Beidou Navigation file");
DEFINE_string(obs,"","Observation file");
DEFINE_string(out,"","Output file");


using namespace std;
#define PROGNAME    "rnx2rtkp"          /* program name */
#define MAXFILE     8                   /* max number of input files */


int load_opt(const char *path, prcopt_t *prcopt, solopt_t *solopt, filopt_t *filopt) {

    /*修改部分缺省配置*/
    prcopt->mode = PMODE_KINEMA;
    prcopt->navsys = SYS_GPS | SYS_CMP;
    prcopt->refpos = 1;
    prcopt->glomodear = 1;
    solopt->timef = 0;
    sprintf(solopt->prog, "%s ver.%s", PROGNAME, VER_RTKLIB);
    sprintf(filopt->trace, "%s.trace", PROGNAME);
    resetsysopts();/*重置上面的配置*/
    if (!loadopts(path, sysopts)) return -1;
    getsysopts(prcopt, solopt, filopt);

    if (prcopt->mode != PMODE_SINGLE) { /* for precise positioning */
        prcopt->ionoopt = IONOOPT_BRDC;
        prcopt->tropopt = TROPOPT_SAAS;
    }
    return 0;
}

int read_rinex_obs_nav_files(const char *obs_path, const char *nav_path,const char *nav2_path, obs_t *obss, nav_t *nav, prcopt_t *prcopt) {
    int rcv = 1;/*接收机编号*/
    gtime_t ts = {0, 0}, te;/*起止时间*/
    int tint = 0;
    const char *opt = "";/**/
    static sta_t stas[MAXRCV];
    int res;
    obss->data = nullptr;
    obss->n = obss->nmax = 0;
    nav->eph = nullptr;
    nav->n = nav->nmax = 0;
    nav->geph = nullptr;
    nav->ng = nav->ngmax = 0;
    nav->seph = nullptr;
    nav->ns = nav->nsmax = 0;

    res = readrnxt(obs_path, rcv, ts, te, tint, opt, obss, nav, stas);
    if (res != 1) {
        trace(1, "obs_file read failed  return code = %d\n", res);
        return 0;
    }
    rcv += 1;
    res = readrnxt(nav_path, rcv, ts, te, tint, opt, obss, nav, stas + 1);
    if (res != 1) {
      trace(2, "eph_file failed return code = %d\n", res);
        return -1;
    };
    res = readrnxt(nav2_path, rcv, ts, te, tint, opt, obss, nav, stas + 1);
    if (res != 1) {
      trace(2, "eph_file failed return code = %d\n", res);
        return -1;
    };
    if (nav->n <= 0 && nav->ng <= 0 && nav->ns <= 0) {
//        checkbrk("error : no nav data");
trace(1, "no nav data", nav->n);
        return -1;
    }
    trace(1, "nav data count = %d", nav->n);
    /* sort observation data */
    sortobs(obss);
    /* delete duplicated(重复的) ephemeris */
    uniqnav(nav);
    return 0;
}

int main(int argc, char *argv[]) {

    google::InitGoogleLogging(".");
    google::LogToStderr();
    prcopt_t prcopt = prcopt_default;
    solopt_t solopt = solopt_default;
    filopt_t filopt = {""};
    string opt_file = "../rtklib_spp.conf";
    /*默认配置选项 定义于rtkcmn.c*/
    if (load_opt(opt_file.c_str(), &prcopt, &solopt, &filopt)) {
        LOG(ERROR) << "opt file load failed";
        return -1;
    }
    const std::string gps_eph_file = fLS::FLAGS_gps;
    const std::string bds_eph_file = fLS::FLAGS_bds;
    const std::string obs_file = fLS::FLAGS_obs;
    const std::string out_file = fLS::FLAGS_out;


    nav_t nav;
    static obs_t obss = {0};

    read_rinex_obs_nav_files(obs_file.c_str(), gps_eph_file.c_str(), bds_eph_file.c_str(), &obss, &nav, &prcopt);


    int i, nobs;
    int n, svh[MAXOBS];
    double *rs, *azel_, *resp;
    double *dts, *var;

    obsd_t obs[MAXOBS * 2]; /* for rover and base */

    /*写入文件头*/
    ofstream ss;
    ss.open(out_file);
    if (!ss.good()) { LOG(FATAL) << "file create failed"; }
    char buff[2048];
    ss << "# program      :" << argv[0] << endl;
    ss << "# obs file     :" << obs_file << endl;
    ss << "# nav file     :" << gps_eph_file << endl;
    ss << "# start time   :" << time_str(obss.data->time, 1) << "\t UTC+0" << endl;
    ss << "# version      :" << "v0.1" << endl;
    sprintf(buff,
            " %5s %12s %3s %13s %13s %13s %13s %13s %13s %5s %5s %18s %18s %18s %15s %15s %15s %6s %15s %5s %8s %8s %8s",
            "week", "sec", "prn",
            "pseudorange1", "pseudorange2",
            "carrier1", "carrier2", "doppler1", "doppler2", "snr1", "snr2",
            "sat-posx", "sat-posy", "sat-posz", "sat-velx", "sat-vely", "sat-velz", "var", "bias", "drift",
            "azimuth",
            "elevation",
            "sat-num"
    );
    const char *formats = " %5d" /*week*/
                          " %12.5f" /* time */
                          " %3d" /* prn*/
                          " %13.3f %13.3f %13.3f %13.3f %13.6f %13.6f" /*p1 p2 l1 l2 d1 d2*/
                          " %5s %5s" /* snr1 snr2*/
                          " %18.6f %18.6f %18.6f"/*sat pos*/
                          " %15.6f %15.6f %15.6f"/*sat vel*/
                          " %6.3f"/*sat position and clock error variance (m^2)*/
                          " %15.3f %5.1f"/* clock bias drift*/
                          " %8.4f" /*azimuth*/
                          " %8.3f" /*elevation*/
                          " %8d\n";/*satellite number*/


    ss << buff << endl;

    int nu, nr;
    double gps_sec, pos[3];
    int gps_week, vsat[MAXOBS] = {0};
    sol_t sol = {{0}};
    rtk_t rtk;
    rtkinit(&rtk, &prcopt);
    char msg[128] = "";
    while ((nobs = inputobs(obs, 0, &prcopt, &obss)) > 0) {
        /* exclude satellites */
        for (i = n = 0; i < nobs; i++) {
            if ((satsys(obs[i].sat, 0) & prcopt.navsys) &&
                prcopt.exsats[obs[i].sat - 1] != 1)
                obs[n++] = obs[i];
        }
        if (n <= 0) {
          LOG(INFO)  << "time " << time_str(obs[i].time, 3) << " , no gnss measurement ";
            continue;
        }

        rtk.sol.stat = SOLQ_NONE;
        rtk.sol.time = obs[0].time;
        /* count rover/base station observations */
        for (nu = 0; nu < n && obs[nu].rcv == 1; nu++);
        for (nr = 0; nu + nr < n && obs[nu + nr].rcv == 2; nr++);
        dts = mat(2, nu);
        var = mat(1, nu);
        rs = mat(6, nu);
        azel_ = zeros(2, nu);
        resp = mat(1, nu);
        satposs(obs[0].time, obs, nu, &nav, prcopt.sateph, rs, dts, var, svh);

        pntpos(obs, nu, &nav, &prcopt, &sol, azel_, rtk.ssat, msg);


        for (i = 0; i < nu && i < MAXOBS; i++) {
            gps_sec = time2gpst(obs[i].time, &gps_week);
            sprintf(buff,
                    formats,
                    gps_week,
                    gps_sec,
                    obs[i].sat,
                    obs[i].P[0], obs[i].P[1], obs[i].L[0], obs[i].L[1], obs[i].D[0], obs[i].D[1],
                    "NaN", "NaN",/*snr 没有*/
                    rs[i * 6], rs[1 + i * 6], rs[2 + i * 6],
                    rs[3 + i * 6], rs[4 + i * 6], rs[5 + i * 6],
                    var[i],
                    dts[i * 2] * 1e9, dts[1 + i * 2] * 1e9,
                    azel_[2 * i] * R2D, azel_[2 * i + 1] * R2D,
                    nu);
            ss << buff;
            /*for debug 计算卫星到用户的距离，和伪距做比对*/
            double pr[3], p = 0;
            for (int idx = 0; idx < 3; idx++) {
                pr[idx] = sol.rr[idx] - rs[i * 6 + idx];
                p += (pr[idx] * pr[idx]);
            }
            p = sqrt(p) - dts[i * 2] * CLIGHT;/*修补钟差*/
            LOG(INFO)  << "p=" << p << "\t pr=" << obs[i].P[0] << "\t diff=" << p - obs[i].P[0];
        }
//        ecef2pos(sol.rr, pos);
//        logi <<gps_sec << std::setprecision(10) << "\t" << sol.rr[0] << "\t" << sol.rr[1] << "\t"<< sol.rr[2];
        free(rs);
        free(dts);
        free(var);
        free(azel_);
//        free()
    };
    ss.close();
    freenav(&nav, 0xff);
    freeobs(&obss);
    LOG(INFO) << "finished" << endl;
    return 0;
}
